﻿import cv2
import numpy as np
import time
import math
import global_var as glv
import visual_function as vf
from basic_game import basic_game

class bridge(basic_game):
    def __init__(self, running = False):
        ###云台初始化###
        self.PWM_Y = 1800 #当前云台值，及初始化最小值
        basic_game.__init__(self, running)
        ###标志位###
        self.throughGate = False #已通过该关卡
        self.get_color = False #是否获得目标颜色区块
        self.on_bridge = False
        ###阈值###
        self.target_color = "green"
        self.perY = 100
        self.PWM_H_UpY = 1700 #抬头调位姿云台角度
        self.PWM_H_DownY = 2200 #低头调位姿云台角度
        self.PWM_MaxY = 2300 #跟踪目标体最低角度
        self.PWM_startY = 2200 #开始根据目标物体左右平移自身位姿的云台高度
        self.minCenter = self.resize_width/2 - self.resize_width/8 #中心区域左边界
        self.maxCenter = self.resize_width/2 + self.resize_width/8 #中心区域右边界
        self.threahold_bridge_near = self.resize_height - self.resize_height/3
        self.threahold_change_PWM = self.resize_height - self.resize_height/3 #在云台未到目标值时目标接近需要更高云台的阈值
        ###变量####
        self.lineList = None
        self.center_x = None


    def logic_bridge(self):
        if self.get_color:
            self.get_color = False
            #未在桥上
            if self.on_bridge is False:
                #距离目标物已经足够近，检测结果比较可靠，可以根据目标物相对位置调整自身位置
                if self.PWM_Y >= self.PWM_startY:
                    if self.center_x >= self.minCenter and self.center_x <= self.maxCenter:
                        #物体在正中位置
                        if self.PWM_Y >= self.PWM_MaxY:             
                            if self.lineList["down_y"]> self.threahold_bridge_near:
                                self.state = "go_straight"
                                basic_game.execute(self,executeNum == 2)
                                self.on_bridge = True
                            else:
                                self.state = "go_straight"
                                basic_game.execute(self)
                                self.state = "change_PWM"
                                basic_game.execute(self, self.PWM_H_UpY)
                                self.headUp = True
                        else:
                            if self.lineList["down_y"] >= self.threahold_change_PWM:
                                self.state = "change_PWM"
                                self.PWM_Y += self.perY
                                if self.PWM_Y >= self.PWM_MaxY:
                                    self.PWM_Y = self.PWM_MaxY
                                basic_game.execute(self, self.PWM_Y)
                            else:
                                self.state = "go_straight"
                                basic_game.execute(self)
                                self.state = "change_PWM"
                                basic_game.execute(self, self.PWM_H_UpY)
                                self.headUp = True
                    elif self.center_x < self.minCenter:
                        self.state = "strafe_left"
                        basic_game.execute(self)
                    elif self.center_x > self.maxCenter:
                        self.state = "strafe_right"
                        basic_game.execute(self)
                else:
                    if self.lineList["down_y"] >= self.threahold_change_PWM:
                        self.state = "change_PWM"
                        self.PWM_Y += self.perY
                        if self.PWM_Y >= self.PWM_MaxY:
                            self.PWM_Y = self.PWM_MaxY
                        basic_game.execute(self, self.PWM_Y)
                    else:
                        self.state = "go_straight"
                        basic_game.execute(self)
                        self.state = "change_PWM"
                        basic_game.execute(self, self.PWM_H_UpY)
                        self.headUp = True
            else:
                if self.center_x >= self.minCenter and self.center_x <= self.maxCenter:
                    self.state = "go_straight"
                    basic_game.execute(self)
                    self.state = "change_PWM"
                    self.headUp = True
                    basic_game.execute(self,self.PWM_H_UpY)
                elif self.center_x < self.minCenter:
                    self.state = "turn_left"
                    basic_game.execute(self)
                elif self.center_x > self.maxCenter:
                    self.state = "turn_right"
                    basic_game.execute(self)
        else:
            if self.on_bridge and self.throughGate is False:
                self.throughGate = True
            else:
                self.state = "go_straight"
                basic_game.execute(self)

    def run(self, hsvImage, outImage = None):
        if self.headUp:
            self.get_line, self.lines = vf.getLineByColor(hsvImage, 'black',outImage = outImage)
            if outImage is not None:
                cv2.imshow("stepframe",outImage)
                cv2.waitKey(1)
            basic_game.logic_position_up(self)
        elif self.headDown:
            self.get_line, self.lines = vf.getLineByColor(hsvImage, 'black',outImage = outImage)
            if outImage is not None:
                cv2.imshow("stepframe",outImage)
                cv2.waitKey(1)
            basic_game.logic_position_down(self)
        else:
            self.get_color, self.lineList, self.center_x = vf.getAreaByColor(hsvImage, self.target_color, outImage)
            if outImage is not None:
                cv2.imshow("stepframe",outImage)
                cv2.waitKey(1)
            bridge.logic_bridge(self)
            if self.throughGate:
                return True
        return False
        